#include <math.h>
#include "KF.h"

//定义俯仰角的卡尔曼滤波器
KalmanFilter kf_pitch;
//定义横滚角的卡尔曼滤波器
KalmanFilter kf_roll;
//定义四元数数组
volatile float q4[4] = {1, 0, 0, 0};
//定义加速度计的初始值，以g为单位
float acc_init = 0;

void Kalman_Init(KalmanFilter *kf, float q, float r, float p, float x);
void Kalman_Update(KalmanFilter *kf, float measurement, float estimated_value
    , float measurement_var);
void Normalize_Quaternion(float *qt);

//初始化卡尔曼滤波器
void Kalman_Init(KalmanFilter *kf, float q, float r, float p, float x)
{
    kf->q = q; //过程噪声协方差
    kf->r = r; //测量噪声协方差
    kf->p = p; //估计误差协方差
    kf->x = x; //估计值
}

//卡尔曼滤波器的单次更新
//输入参数为卡尔曼滤波器的指针、测量值和估计值、测量值的方差
//输出参数为空
void Kalman_Update(KalmanFilter *kf, float measurement, float estimated_value
    , float measurement_var)
{
    //更新测量值的方差
    kf->r = measurement_var;
    //更新卡尔曼滤波的估计值
    kf->x = estimated_value;

    //更新估计误差协方差
    kf->p = kf->p + kf->q;
    //计算卡尔曼增益
    kf->k = kf->p / (kf->p + measurement_var);
    //更新估计值
    kf->x = kf->x + kf->k * (measurement - kf->x);
    //更新估计误差协方差
    kf->p = (1 - kf->k) * kf->p;
}

//整个算法初始化
void KF_Init(float *acc_init_value)
{
    //将加速度计的初始值赋值给acc_init
    acc_init = *acc_init_value;

    //将四元数的初始值赋值为单位四元数
    q4[0] = 1;
    q4[1] = 0;
    q4[2] = 0;
    q4[3] = 0;

    //初始化俯仰角的卡尔曼滤波器
    Kalman_Init(&kf_pitch, GYRO_VAR, ACC_VAR, 1, 0);
    //初始化横滚角的卡尔曼滤波器
    Kalman_Init(&kf_roll, GYRO_VAR, ACC_VAR, 1, 0);
}

//归一化四元数
//输入参数为四元数的指针
//输出参数为空
void Normalize_Quaternion(float *qt)
{
    //计算四元数的模
    float norm = sqrtf(qt[0] * qt[0] + qt[1] * qt[1] + qt[2] * qt[2] + qt[3] * qt[3]);
    //归一化四元数
    for (int i = 0; i < 4; i++)
    {
        qt[i] /= norm;
    }
}

//执行姿态解算
//输入参数为2个float*
//第一个float*为本次测量的6个值
//加速度计的值为前3个float，陀螺仪的值为后3个float
//第二个float*为本次更新的结果值，长度为3个float，分别为俯仰角、横滚角和航向角
//先存储加速度计和陀螺仪的本次测量值
float acc[3] = {0};
float gyro[3] = {0};
float q_dot[4] = {0};
void KF_Update(float *measured_value,float *result_value)
{

    for (int i = 0; i < 3; i++)
    {
        acc[i] = measured_value[i];
        gyro[i] = measured_value[i + 3];
        //对角速度做一个处理，在阈值之内的认为是噪声，设定为0
        if (fabsf(gyro[i]) < GYRO_THRESHOLD)
        {
            gyro[i] = 0;
        }
    }

    //--步骤1，确定当前角度值

    //定义本次的俯仰角、横滚角的测量值，基于加速度计的值
    float pitch_measured = atan2f(acc[1], sqrtf(acc[0] * acc[0] + acc[2] * acc[2])) * 180 /IMU_M_PI;
    float roll_measured = atan2f(-acc[0], sqrtf(acc[1] * acc[1] + acc[2] * acc[2])) * 180 /IMU_M_PI;


    //将本次测量的陀螺仪的值转换为弧度制
    float gyro_x = gyro[0] * IMU_M_PI / 180.0f;
    float gyro_y = gyro[1] * IMU_M_PI / 180.0f;
    float gyro_z = gyro[2] * IMU_M_PI / 180.0f;

    //将弧度制的值乘以时间间隔，得到本次测量的积分值
    gyro_x *= TIME_INTERVAL;
    gyro_y *= TIME_INTERVAL;
    gyro_z *= TIME_INTERVAL;

    //将陀螺仪的值转换为四元数
    q_dot[0] = 0.5f * (-q4[1] * gyro_x - q4[2] * gyro_y - q4[3] * gyro_z);
    q_dot[1] = 0.5f * (q4[0] * gyro_x + q4[2] * gyro_z - q4[3] * gyro_y);
    q_dot[2] = 0.5f * (q4[0] * gyro_y - q4[1] * gyro_z + q4[3] * gyro_x);
    q_dot[3] = 0.5f * (q4[0] * gyro_z + q4[1] * gyro_y - q4[2] * gyro_x);

    //更新四元数
    q4[0] += q_dot[0];
    q4[1] += q_dot[1];
    q4[2] += q_dot[2];
    q4[3] += q_dot[3];

    //归一化四元数
    Normalize_Quaternion(&q4[0]);

    //计算由本次四元数所得出的俯仰角、横滚角和航向角
    float roll = asinf(2.0f * (q4[0] * q4[2] - q4[1] * q4[3])) * 180.0f / IMU_M_PI;  // 俯仰角（Pitch）
    float pitch = atan2f(2.0f * (q4[0] * q4[1] + q4[2] * q4[3]), q4[0]*q4[0] - q4[1]*q4[1] - q4[2]*q4[2] + q4[3]*q4[3]) * 180.0f / IMU_M_PI; // 横滚角（Roll）
    float yaw = atan2f(2.0f * (q4[0] * q4[3] + q4[1] * q4[2]), q4[0]*q4[0] + q4[1]*q4[1] - q4[2]*q4[2] - q4[3]*q4[3]) * 180.0f / IMU_M_PI; // 偏航角（Yaw）


    //---步骤2，执行滤波

    //计算本次的加速度计的合加速度
    float acc_magnitude = sqrtf(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
    //计算合加速度和初始加速度的标准差
    float acc_std = acc_init - acc_magnitude;
    //定义本次卡尔曼滤波融合当中测量值的方差值，即将加速度方差乘以一个权重
    float acc_var =  ACC_VAR + fabsf(acc_std) * ACC_KF_WEIGHT;

    //执行卡尔曼滤波
    //更新俯仰角的卡尔曼滤波器
    Kalman_Update(&kf_pitch, pitch_measured, pitch, acc_var);
    //更新横滚角的卡尔曼滤波器
    Kalman_Update(&kf_roll, roll_measured, roll, acc_var);
    //将滤波后的值赋值给俯仰角和横滚角
    pitch = kf_pitch.x;
    roll = kf_roll.x;

    //---步骤3，执行反馈，将本次的计算出来的俯仰角、横滚角和航向角转换为四元数

    // 定义三个角度转换为弧度制
    float pitch_rad = pitch * IMU_M_PI / 180.0f;
    float roll_rad = roll * IMU_M_PI / 180.0f;
    float yaw_rad = yaw * IMU_M_PI / 180.0f;

    // 计算半角的正弦和余弦
    float cy = cosf(yaw_rad * 0.5f);
    float sy = sinf(yaw_rad * 0.5f);
    float cp = cosf(roll_rad * 0.5f);
    float sp = sinf(roll_rad * 0.5f);
    float cr = cosf(pitch_rad * 0.5f);
    float sr = sinf(pitch_rad * 0.5f);

    // 计算四元数 (ZYX顺序)
    static float q_temp[4] = {0};
    q_temp[0] = cr * cp * cy + sr * sp * sy;  // w
    q_temp[1] = sr * cp * cy - cr * sp * sy;  // x
    q_temp[2] = cr * sp * cy + sr * cp * sy;  // y
    q_temp[3] = cr * cp * sy - sr * sp * cy;  // z

    Normalize_Quaternion(&q_temp[0]);
    //以差量方式更新四元数
    for (int i = 0; i < 4; i++)
    {
        // q_error = q_temp[i] - q4[i];
        // q4[i] += q_error * QUAT_KF_WEIGHT;
        q4[i] = q_temp[i];
    }

    //---步骤4，将本次的俯仰角、横滚角和航向角赋值给结果值，单位为角度
    result_value[0] = pitch;
    result_value[1] = roll ;
    result_value[2] = yaw  ;

    return;
}
